### Project 2 MPU6050 Attitude Fusion Solution (Madgwick Filter)
***1.Introduction to Attitude***
**Attitude** refers to the orientation of an object in 3D space. The MPU6050 measures this using three angles known as **Euler Angles**:
- **Roll**: Rotation around the X-axis (tilting left/right).
- **Pitch**: Rotation around the Y-axis (tilting forward/backward).
- **Yaw**: Rotation around the Z-axis (turning left/right).
***Why use Quaternions?***
While Euler angles are easy to understand, they suffer from **Gimbal Lock** (loss of one degree of freedom at certain angles). To solve this, we use **Quaternions**—a mathematical representation using four numbers ($q_0, q_1, q_2, q_3$) that simplify calculations and ensure smooth rotation tracking for drones and robots.
***2. The Attitude Acquisition Process***
The process follows a specific workflow to ensure raw sensor "noise" is filtered into clean data.
Step 1: Convert Accelerometer Data
Raw data from the ADC must be converted into G-force ($g$).
Formula: $Acceleration = \frac{Original Value}{Resolution}$
```c
mpu.readAccelData(accelCount);
aRes = mpu.getAres(); // Get LSB/g resolution
ax = (float)accelCount[0] * aRes;
ay = (float)accelCount[1] * aRes;
az = (float)accelCount[2] * aRes;
```
Step 2: Convert Gyroscope Data
Convert raw angular speed into Degrees per Second ($°/s$).
Formula: $Angular Speed = \frac{Original Value}{Resolution}$
```c
mpu.readGyroData(gyroCount);
gRes = mpu.getGres(); // Get LSB/(°/s) resolution
gyrox = (float)gyroCount[0] * gRes;
gyroy = (float)gyroCount[1] * gRes;
gyroz = (float)gyroCount[2] * gRes;
```
Step 3: Calculate Integration Interval
The algorithm needs to know exactly how much time has passed between updates (`deltat`) to integrate the movement accurately.
```c
Now = micros();
deltat = ((Now - lastUpdate) / 1000000.0f); // Convert microseconds to seconds
lastUpdate = Now;
```
Step 4: Madgwick Quaternion Update
Before fusion, we convert gyroscope data to **Radians** because the math requires circular measurement ($Radians = Degrees \times \frac{\pi}{180}$).
```c
// Convert to Radians
gyrox *= (PI / 180.0f);
gyroy *= (PI / 180.0f);
gyroz *= (PI / 180.0f);
// Update Quaternion using Madgwick Filter
MadgwickQuaternionUpdate(ax, ay, az, gyrox, gyroy, gyroz);
```
**Inside the Filter:**
1. **Normalize** the accelerometer vector.
2. **Compute the Gradient** via the Jacobian matrix to find the direction of the error.
3. **Correct Gyro Bias** to remove "Null Shift" (drift).
4. **Integrate** the rate of change to find the new Quaternion state.
------
4. Converting Quaternions to Euler Angles
Finally, we convert the abstract Quaternions back into human-readable degrees.
```c
// Mathematical Conversion
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
// Convert Radians back to Degrees for display
pitch *= 180.0f / PI;
roll *= 180.0f / PI;
```
***3.code***
Based on the above principles, we can write the following code. After uploading the code, we open the serial monitor and serial plotter respectively. By changing the glove's posture, we can obtain real-time posture data from the glove.
```c
#include "Wire.h"
#include "MPU6050.h"
MPU6050lib mpu;
// --- Core Attitude Variables ---
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // Quaternion buffer [qw, qx, qy, qz]
float pitch, roll, yaw; // Calculated Euler angles (Degrees)
float ax, ay, az, gx, gy, gz; // Converted physical values (g and rad/s)
int16_t aRaw[3], gRaw[3]; // Raw ADC data from sensors
// --- Madgwick Filter Parameters ---
// Beta controls the filter gain. High beta = fast tracking; Low beta = smooth data.
float beta = 0.1f;
float deltat = 0.0f; // Integration time interval
uint32_t lastUpdate = 0;
void setup() {
Wire.begin();
// Set Baud Rate to 9600 for standard communication
Serial.begin(9600);
// Initialize MPU6050
uint8_t c = mpu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
if (c == 0x68) {
Serial.println(F("MPU6050 Online. Starting Calibration..."));
float gBias[3], aBias[3];
// Keep the sensor flat and still during calibration
mpu.calibrateMPU6050(gBias, aBias);
mpu.initMPU6050();
Serial.println(F("Calibration Complete. Format: Pitch, Roll, Yaw"));
} else {
Serial.print(F("Connection Error: 0x"));
Serial.println(c, HEX);
while (1); // Halt if sensor not found
}
}
void loop() {
// 1. Check if Data Ready interrupt is set
if (mpu.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {
// Read Accelerometer and convert to g
mpu.readAccelData(aRaw);
float aRes = mpu.getAres();
ax = (float)aRaw[0] * aRes;
ay = (float)aRaw[1] * aRes;
az = (float)aRaw[2] * aRes;
// Read Gyroscope and convert to Radians per Second
mpu.readGyroData(gRaw);
float gRes = mpu.getGres();
gx = (float)gRaw[0] * gRes * PI / 180.0f;
gy = (float)gRaw[1] * gRes * PI / 180.0f;
gz = (float)gRaw[2] * gRes * PI / 180.0f;
// 2. Update Integration Time (dt)
uint32_t now = micros();
deltat = (now - lastUpdate) / 1000000.0f;
lastUpdate = now;
// 3. Update Quaternion using Madgwick algorithm
MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz);
// 4. Convert Quaternion to Euler Angles (Tait-Bryan convention)
// Yaw: Turning left/right (drift-prone without magnetometer)
// Pitch: Forward/Backward tilt
// Roll: Side-to-side tilt
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
// 5. Convert Radians to Degrees
pitch *= 180.0f / PI;
roll *= 180.0f / PI;
yaw *= 180.0f / PI;
// 6. Print data for Serial Monitor/Plotter
Serial.print("Pitch:"); Serial.print(pitch);
Serial.print(",");
Serial.print("Roll:"); Serial.print(roll);
Serial.print(",");
Serial.print("Yaw:"); Serial.println(yaw);
}
// Control the loop frequency (~100Hz)
delay(10);
}
// --- Madgwick Filter Implementation ---
// Efficiently fuses accelerometer and gyroscope data to produce stable attitude.
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) {
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
float norm;
float f1, f2, f3;
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
float qDot1, qDot2, qDot3, qDot4;
float hatDot1, hatDot2, hatDot3, hatDot4;
// Normalize accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return;
norm = 1.0f / norm;
ax *= norm; ay *= norm; az *= norm;
// Pre-calculate common variables
float _2q1 = 2.0f * q1, _2q2 = 2.0f * q2, _2q3 = 2.0f * q3, _2q4 = 2.0f * q4;
// Compute objective function and Jacobian
f1 = _2q2 * q4 - _2q1 * q3 - ax;
f2 = _2q1 * q2 + _2q3 * q4 - ay;
f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
J_11or24 = _2q3; J_12or23 = _2q4; J_13or22 = _2q1; J_14or21 = _2q2;
J_32 = 2.0f * J_14or21; J_33 = 2.0f * J_11or24;
// Compute the gradient (matrix multiplication)
hatDot1 = J_14or21 * f2 - J_11or24 * f1;
hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
hatDot3 = J_12or23 * f2 - J_33 * f3 - J_13or22 * f1;
hatDot4 = J_14or21 * f1 + J_11or24 * f2;
// Normalize the gradient
norm = 1.0f / sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
hatDot1 *= norm; hatDot2 *= norm; hatDot3 *= norm; hatDot4 *= norm;
// Compute quaternion derivative (rate of change)
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz);
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy);
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx);
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx);
// Integrate to find next attitude state
q1 += (qDot1 - (beta * hatDot1)) * deltat;
q2 += (qDot2 - (beta * hatDot2)) * deltat;
q3 += (qDot3 - (beta * hatDot3)) * deltat;
q4 += (qDot4 - (beta * hatDot4)) * deltat;
// Normalize final quaternion
norm = 1.0f / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
q[0] = q1 * norm; q[1] = q2 * norm; q[2] = q3 * norm; q[3] = q4 * norm;
}
```
***Instructions for Use:***
1. **Coordinate System**: Understand the axis layout of the MPU6050 to correctly interpret Pitch and Roll.
2. **Serial Monitor**: Open the Arduino Serial Monitor and set the baud rate to **9600**. You will see the numeric values.
3. **Serial Plotter**: For a better experience, go to **Tools > Serial Plotter**. You will see smooth, colorful lines representing your movement in real-time.
4. **Calibration**: Ensure the sensor is on a flat, stable surface when you power it on or reset it, so the calibration can correctly remove the sensor bias.